#include "stdafx.h"
#include "World.h"
#include <algorithm>
using namespace std;

void GameObject::syncPhysicsAndGraphics()
{
	if(!node) return;
	btTransform trans;
	body->getMotionState()->getWorldTransform(trans);
    btQuaternion rot = trans.getRotation();
    btVector3 pos = trans.getOrigin();
    node->setOrientation(rot.w(), rot.x(), rot.y(), rot.z());
    node->setPosition(pos.x(), pos.y(), pos.z());
}

World::World(Ogre::SceneManager* sceneManager):
	pause(false),
	step(0),
	sceneManager(sceneManager)
{
	physEngine.reset(new PhysEngine());
}

GameObject* World::addObject(Ogre::SceneNode* node, btCollisionShape* shape, const btTransform& transform, float mass, bool temp)
{
	btRigidBody* body = physEngine->createBody(shape, transform, mass);
	GameObject* obj = new GameObject(node, body, temp);
	objects.push_back(obj);
	return obj;
}

void World::removeObject(GameObject* object)
{
	objects.erase(remove(objects.begin(), objects.end(), object), objects.end());
	if(object->node) sceneManager->destroySceneNode(object->node);
	physEngine->removeBody(object->body);
	delete object;
}

void World::addJoint(btTypedConstraint* joint, bool disableCollision)
{
	physEngine->addJoint(joint, disableCollision);
}

void World::update(bool syncPhysicsAndGraphics)
{
	if(!pause)
	{
		physEngine->step(1.f/60.f);
		step++;
	}
	if(syncPhysicsAndGraphics)
	{
		for(size_t i = 0; i < objects.size(); i++)
		{	
			objects[i]->syncPhysicsAndGraphics();
		}
	}
}

Ogre::SceneManager* World::getSceneManager()
{
	return sceneManager;
}

void World::reset(bool syncPhysicsAndGraphics)
{
	for(vector<GameObject*>::iterator i = objects.begin(); i != objects.end();)
	{
		GameObject* obj = *i;
		if(obj->temp)
		{
			i = objects.erase(i);
			if(obj->node) sceneManager->destroySceneNode(obj->node);
			physEngine->removeBody(obj->body);
			delete obj;
		}
		else
		{
			i++;
		}
	}
	step = 0;
	physEngine->reset();
	if(syncPhysicsAndGraphics)
	{
		for(size_t i = 0; i < objects.size(); i++)
		{	
			objects[i]->syncPhysicsAndGraphics();
		}
	}
}

size_t World::getStep() const
{
	return step;
}

World::~World()
{

}